//
// Created by sxy on 2021/11/8.
//

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <nav_msgs/Odometry.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <math.h>
#include "map_update/map_update.h"
#include <pcl/filters/crop_hull.h>
#include <pcl/surface/concave_hull.h>
#include <ros/package.h>
#include <yaml-cpp/yaml.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>

using namespace std;

//原始点云地图
pcl::PointCloud<pcl::PointXYZ>::Ptr origin_map(new pcl::PointCloud<pcl::PointXYZ>);
//当前局部点云地图
pcl::PointCloud<pcl::PointXYZ>::Ptr cur_map(new pcl::PointCloud<pcl::PointXYZ>);
//上一个分割时刻的位置，当前位置，两个位置超过阈值才会分割子地图
vector<double> last_pose(3);
vector<double> cur_pose(3);
//分割子地图的距离阈值    最后分割出的矩形长为len的二倍，宽为seg_limit
double seg_limit;
double len;
//是否已经被初始化
bool if_init=false;
void callback(sensor_msgs::PointCloud2ConstPtr cur_velodyne_points,nav_msgs::OdometryConstPtr pose);
std::string path;
Cfg cfg;

int main(int argc,char** argv){
    ros::init(argc,argv,"map_update");
    ros::NodeHandle nh;
    path = ros::package::getPath("lidar_localization");

    //读取地图更新配置文件，里边有各种路径以及所需的参数
    string config_path=path+"/config/map_update/update.yaml";   //读取配置文件
    YAML::Node config_node=YAML::LoadFile(config_path);
    string map_path=config_node["map_path"].as<string>();


    string cfg_path=config_node["cfg_path"].as<string>();
    cfg_path=path+cfg_path;
    readCSGconfig(cfg,cfg_path);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_origin_before(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cur_before(new pcl::PointCloud<pcl::PointXYZ>);

    seg_limit=config_node["seg_limit"].as<double>();
    len=config_node["len"].as<double>();
    //读取地图点云
    pcl::io::loadPCDFile(map_path,*origin_map);


    //对齐两个话题的时间戳以保证点云和其位姿对应
    message_filters::Subscriber<sensor_msgs::PointCloud2> pointcloud_sub(nh,"/current_scan",2);  //激光雷达点云话题
    message_filters::Subscriber<nav_msgs::Odometry> pose_sub(nh,"/laser_localization",2);        //定位结果，与雷达点云对应

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> MySyncPolicy;
    // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), pointcloud_sub, pose_sub);
    sync.registerCallback(boost::bind(&callback, _1, _2));
    ros::spin();
    return 0;
}
void callback(sensor_msgs::PointCloud2ConstPtr cur_velodyne_points,nav_msgs::OdometryConstPtr pose){
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_input;
    pcl::fromROSMsg(*cur_velodyne_points,*cloud_input);
    if(!if_init){
        //如果没有初始化的话，初始化last_pose
        last_pose[0]=pose->pose.pose.position.x;
        last_pose[1]=pose->pose.pose.position.y;
        last_pose[2]=pose->pose.pose.position.z;
        //叠加点云
        (*cur_map)+=(*cloud_input);
    }else{
        (*cur_map)+=(*cloud_input);
        //首先获取当前位姿
        cur_pose[0]=pose->pose.pose.position.x;
        cur_pose[1]=pose->pose.pose.position.y;
        cur_pose[2]=pose->pose.pose.position.z;
        //判断当前位置与前一次分割位置的距离，是否大于切割阈值
        double distance=sqrt(pow(cur_pose[0]-last_pose[0],2)+pow(cur_pose[1]-last_pose[1],2)+pow(cur_pose[2]-last_pose[2],2));
        if(distance>seg_limit){
            //如果超出阈值就分割子地图，先获取分割矩形的四个顶点
            vector<double> leftup;
            vector<double> rightup;
            vector<double> rightdown;
            vector<double> leftdown;
            rectangle(len,leftup,rightup,rightdown,leftdown,cur_pose[0],cur_pose[1],last_pose[0],last_pose[1]);

            //下面将原始地图和当前局部地图分割
            //这两个是分割出来的部分，origin是原始地图，cur是当前地图
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_origin_seg(new pcl::PointCloud<pcl::PointXYZ>);
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cur_seg(new pcl::PointCloud<pcl::PointXYZ>);
            //这两个是剩下的部分

            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_origin_remain(new pcl::PointCloud<pcl::PointXYZ>);
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cur_remain(new pcl::PointCloud<pcl::PointXYZ>);

            //切割局部点云
            pcl::PointCloud<pcl::PointXYZ>::Ptr boundingbox_ptr (new pcl::PointCloud<pcl::PointXYZ>);
            boundingbox_ptr->push_back(pcl::PointXYZ(leftup[0], leftup[1], 0));
            boundingbox_ptr->push_back(pcl::PointXYZ(rightup[0], rightup[1],0));
            boundingbox_ptr->push_back(pcl::PointXYZ(rightdown[0], rightdown[1],0));
            boundingbox_ptr->push_back(pcl::PointXYZ(leftdown[0], leftdown[1],0 ));

            /*求上面给出的这个边框点集的凸包*/
            pcl::ConvexHull<pcl::PointXYZ> hull;
            hull.setInputCloud(boundingbox_ptr);
            hull.setDimension(2); /*设置凸包维度*/
            std::vector<pcl::Vertices> polygons; /*用于保存凸包顶点*/
            pcl::PointCloud<pcl::PointXYZ>::Ptr surface_hull (new pcl::PointCloud<pcl::PointXYZ>); /*用于描绘凸包形状*/
            hull.reconstruct(*surface_hull, polygons);

            //    pcl::PointCloud<pcl::PointXYZ>::Ptr map_box (new pcl::PointCloud<pcl::PointXYZ>);
            //    pcl::PointCloud<pcl::PointXYZ>::Ptr map_remain (new pcl::PointCloud<pcl::PointXYZ>);
            pcl::CropHull<pcl::PointXYZ> map_filter;
            //    map_filter.setCropOutside(false);
            map_filter.setDim(2); /*设置维度*/
            map_filter.setHullIndices(polygons); /*封闭多边形顶点*/
            map_filter.setHullCloud(surface_hull); /*封闭多边形形状*/

            pcl::PointCloud<pcl::PointXYZ>::Ptr origin_map_tmp(new pcl::PointCloud<pcl::PointXYZ>);
            *origin_map_tmp=*origin_map;

            //原始点云 修改点云获得分割点
            map_filter.setInputCloud(cur_map);
            map_filter.filter(*cloud_cur_seg);
            map_filter.setInputCloud(origin_map_tmp);
            map_filter.filter(*cloud_origin_seg);

            map_filter.setCropOutside(false);
            map_filter.filter(*cloud_origin_remain);
            //分割局部地图之后，进行地面去除
            pcl::PointCloud<pcl::PointXYZ>::Ptr seg_without_origin(new pcl::PointCloud<pcl::PointXYZ>);   //原始地图不带点云
            pcl::PointCloud<pcl::PointXYZ>::Ptr seg_ground_origin(new pcl::PointCloud<pcl::PointXYZ>);   //原始地图地面
            pcl::PointCloud<pcl::PointXYZ>::Ptr seg_without_cur(new pcl::PointCloud<pcl::PointXYZ>);     //
            pcl::PointCloud<pcl::PointXYZ>::Ptr seg_ground_cur(new pcl::PointCloud<pcl::PointXYZ>);

            deletegroud(cfg,cloud_origin_seg,seg_without_origin,seg_ground_origin);
            deletegroud(cfg,cloud_cur_seg,seg_without_cur,seg_ground_cur);

            //地面点云去处之后先进行配准,获取实时点云到原始地图的转换矩阵   前修改后原始
            Eigen::Matrix4f transform;
            registration(seg_without_cur,seg_without_origin,transform);

            /*
                 * 将实时点云转换到原始地图坐标系  cur origin
                 * 找出来cur相对于origin增加的点，后边加到origin中
                 * 找出来origin相对于cur减少的点，后边从origin中删掉
            */
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cur_tran(new pcl::PointCloud<pcl::PointXYZ>);
            pcl::transformPointCloud(*seg_without_cur,*cloud_cur_tran,transform);

            vector<int> add_index;
            detect_change(seg_without_origin,cloud_cur_tran,add_index);//这里的index是实时点云cur中的
            vector<int> del_index;
            detect_change(cloud_cur_tran,seg_without_origin,del_index);//这里的index是原始点云origin中的

            pcl::PointCloud<pcl::PointXYZ>::Ptr addpoint(new pcl::PointCloud<pcl::PointXYZ>);
            pcl::PointCloud<pcl::PointXYZ>::Ptr delpoint(new pcl::PointCloud<pcl::PointXYZ>);
            for (auto detected : add_index){
                addpoint->points.push_back(seg_without_cur->points[detected]);
            }
            addpoint->width=add_index.size();
            addpoint->height=1;
            addpoint->resize(addpoint->width);

            for (auto detected : del_index){
                delpoint->points.push_back(seg_without_origin->points[detected]);
            }
            delpoint->width=del_index.size();
            delpoint->height=1;
            delpoint->resize(delpoint->width);


            //下面分两步，一是从origin删除点云，二是向origin中添加点云
            //减点云
            clock_t starttime,startime2,endtime,endtime2;
            starttime=clock();
            pcl::ExtractIndices<pcl::PointXYZ> extract;
            extract.setInputCloud(seg_without_origin);
            boost::shared_ptr<vector<int> > delindex_ptr(new vector<int>(del_index));
            extract.setIndices(delindex_ptr);
            extract.setNegative(true);   //true的话 删除传入的索引
            extract.filter(*seg_without_origin);
            endtime=clock();
            cout << "The run time of delete is: " <<(double)(endtime - starttime) / CLOCKS_PER_SEC << "s" << endl;
            //增点云
            pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);

            startime2=clock();
            for (auto detected : add_index){
                output->points.push_back(seg_without_cur->points[detected]);
            }

            output->width=add_index.size();
            output->height=1;
            output->resize(output->width);
            *(seg_without_origin)+=(*output);
            endtime2=clock();

            //因为之前将地图进行了分割并去除了地面，下面将剩下的点云加上，并补上实时的地面以防止需要用到地面点
            *(cloud_origin_remain)+=(*seg_without_origin);
            *(cloud_origin_remain)+=(*seg_without_origin);
            //将更新后的地图保存下来
            string savepath=path+"/slam_data/update_map/updated_map.pcd";
            pcl::io::savePCDFile(savepath,*cloud_origin_remain);
            //将last_pose更新为当前位姿
            last_pose[0]=cur_pose[0];
            last_pose[1]=cur_pose[1];
            last_pose[2]=cur_pose[2];

            cur_map->clear();
        }else{
            //如果没有到达距离阈值，就拼接点云进入下一步
            (*cur_map)+=(*cloud_input);
            //防止点云数越来越多，进行降采样
            pcl::VoxelGrid<pcl::PointXYZ> filter;
            filter.setInputCloud(cur_map);
            // 设置体素栅格的大小为  单位为米
            filter.setLeafSize(0.05f, 0.05f, 0.05f);
            filter.filter(*cur_map);
        }
    }
}







